
% 需要注意，Ia，Ib与theta之间有一定的关系，在扇区判断的时候没有正确的ia,ib符号会导致计算出来的扇区判断参数与输入的theta角所在扇区对不上
function FOC()
    global QN_M;
    QN_M = 16;  % 设置全局变量 QN_M，定义位宽

    PI_Q28 = 843314856;
    global PI;
    PI = PI_Q28 / 2^(28 - QN_M);
    % 初始角度 theta_range 和迭代次数
    % 角度改这
    theta_range = deg2rad(110) * 2^QN_M;
    num_rotations = 20;
    
    Vd = 0;
    Vq = 24 * 2^QN_M;
    % 调用 CORDIC 函数进行正弦和余弦计算
    fprintf('cos() = %.6f;sin() = %.6f;\n',cos( theta_range/2^16 ),sin( theta_range/2^16 ));
    [cos_val, sin_val] = cordic_sin_cos(theta_range, num_rotations);
    % 打印结果
    fprintf('cos = %.6f; sin = %.6f\n', cos_val/2^16, sin_val/2^16);

    [valpha_val, vbeta_val] = repack(cos_val, sin_val, Vd, Vq);
    fprintf('valpha_val = %.6f; vbeta_val = %.6f\n', valpha_val, vbeta_val);

    [va_val, vb_val, vc_val] = reclark(valpha_val, vbeta_val);
    fprintf('va = %.6f; vb = %.6f; vc = %.6f;\n', va_val, vb_val, vc_val);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [I_alpha, I_beta] = clark(12 * 2^QN_M, 1 * 2^QN_M);
    fprintf('I_alpha = %.6f; I_beta = %.6f;\n', I_alpha, I_beta);

    [cos_val, sin_val] = cordic_sin_cos(deg2rad(80), num_rotations);

    [I_d, I_q] = pack(cos_val, sin_val, I_alpha, I_beta);
    fprintf('Vd = %.6f; I_d = %.6f; Vq = %.6f; I_q = %.6f;\n', Vd, I_d, Vq, I_q);

    [block, a,b,c] = sector_judgment( I_alpha, I_beta );
    fprintf('block = %.6f; a = %.6f; b = %.6f; c = %.6f;\n', block, a, b, c);

    [tout,arra,arrb,arrc] = action_duration(block, a, b, c, 24*2^QN_M, 25*2^QN_M, 50*2^QN_M);
    fprintf('tout = %.6f; arra = %.6f; arrb = %.6f; arrc = %.6f;\n', tout, arra, arrb, arrc);

    up_down_count( tout,arra,arrb,arrc );
    % test_pid();
end

% num_rotations: 迭代次数
function [x_cos, y_sin] = cordic_sin_cos(alpha_angle, num_rotations)
    global QN_M;  % 引用全局变量 QN_M
    global PI;
    
    % 定义20个迭代角度，定点格式（Q28）
    ANGLE_Q28 = [210828714; 124459457; 65760959; 33381290; 16755422; 8385879; ...
                 4193963; 2097109; 1048571; 524287; 262144; 131072; 65536; ...
                 32768; 16384; 8192; 4096; 2048; 1024; 512];             

    % 初始化CORDIC增益常数K
    K = 0.607252935008881 * 2^28;
    K = K / 2^(28 - QN_M);  % 调整定点表示
    x = round(K);  % 初始化 x
    y = 0;  % 初始化 y
    theta = alpha_angle;  % 初始角度

    % 定义 π 的 Q28 表示
    ANGLE_90  = PI / 2;
    ANGLE_180 = PI;
    ANGLE_270 = PI * 3 / 2;
    ANGLE_360 = PI * 2;
    quadrant = 1;
    % 角度归一化，将角度调整到 [0, 90] 区间
    if theta > ANGLE_90 && theta <= ANGLE_180
        theta = ANGLE_180 - theta;
        quadrant = 2;
    elseif theta > ANGLE_180 && theta <= ANGLE_270
        theta = theta - ANGLE_180;
        quadrant = 3;
    elseif theta > ANGLE_270 && theta <= ANGLE_360
        theta = ANGLE_360 - theta;
        quadrant = 4;
    end
    % CORDIC 旋转过程
    for cnt = 1:num_rotations
        if theta >= 0.0
            % 正旋转：x 和 y 更新
            temp_x = x - y/2^(cnt-1);
            temp_y = y + x/2^(cnt-1);
            theta = theta - bitshift(ANGLE_Q28(cnt), -(28 - QN_M));  % 更新角度
        else
            % 负旋转：x 和 y 更新
            temp_x = x + y/2^(cnt-1);
            temp_y = y - x/2^(cnt-1);
            theta = theta + bitshift(ANGLE_Q28(cnt), -(28 - QN_M));  % 更新角度
        end
        x = temp_x;
        y = temp_y;
    end

    % 四象限调整，确保 sin 和 cos 的符号正确
    % x = cos; y = sin
    switch quadrant
        case 1
            x_cos = x;
            y_sin = y;
        case 2
            x_cos = -x;
            y_sin = y;
        case 3
            x_cos = -x;
            y_sin = -y;
        case 4
            x_cos = x;
            y_sin = -y;
    end
end

function [valpha, vbeta] = repack(cos_theta,sin_theta, vd, vq)
    global QN_M;
%     Valpha(i) = Vd(i) * cos(theta) - Vq(i) * sin(theta); % 根据反 Park 变换公式计算 Valpha
%     Vbeta(i) = Vd(i) * sin(theta) + Vq(i) * cos(theta);  % 根据反 Park 变换公式计算 Vbeta
    valpha_temp = (vd * cos_theta)/2^QN_M - ( vq * sin_theta )/2^QN_M;
    vbeta_temp  = (vd * sin_theta)/2^QN_M + ( vq * cos_theta )/2^QN_M;
    valpha = valpha_temp;
    vbeta  = vbeta_temp;
end

function [Va,Vb,Vc] = reclark(v_alpha, v_beta)
    global QN_M;
    sqrt3_Q28 = 464943848;
    sqrt3 = sqrt3_Q28 / 2^(28 - QN_M);
    Va = v_alpha;
    Vb = (-v_alpha + (v_beta * sqrt3) / 2^QN_M)/2;
    Vc = (-v_alpha - (v_beta * sqrt3) / 2^QN_M)/2;
end

function [Ialpha, Ibeta] = clark(ia, ib)
    global QN_M;
    sqrt3_Q28 = 464943848;
    sqrt3 = sqrt3_Q28 / 2^(28 - QN_M);
    Ialpha = ia;
    Ibeta  = (ia + 2 * ib) * 2^QN_M / sqrt3;
end

function [Id, Iq] = pack(cos_theta, sin_theta, Ialpha, Ibeta)
    global QN_M;
    Id = Ialpha * cos_theta / 2^QN_M + Ibeta * sin_theta / 2^QN_M;
    Iq = -Ialpha * sin_theta / 2^QN_M + Ibeta * cos_theta / 2^QN_M;
end

% 扇区判断
function [N, A, B, C] = sector_judgment(Valpha, Vbeta)
    global QN_M;
    sqrt3_Q28 = 464943848;
    sqrt3 = sqrt3_Q28 / 2^(28 - QN_M);
    sqrt3_2 = sqrt3 / 2; % 根号3除2
    a_buf = Vbeta;
    b_buf = sqrt3_2 * Valpha / 2^QN_M - Vbeta / 2;
    c_buf = -(sqrt3_2 * Valpha / 2^QN_M) - Vbeta / 2;

    if ( 0 < a_buf )
        a_reg = 1;
    else
        a_reg = 0;
    end
    if ( 0 < b_buf )
        b_reg = 1;
    else
        b_reg = 0;
    end
    if ( 0 < c_buf )
        c_reg = 1;
    else
        c_reg = 0;
    end
    n_buf = 4 * c_reg + 2 * b_reg + a_reg;

    N = n_buf;
    A = a_buf;
    B = b_buf;
    C = c_buf;
end

% 计算作用时长并转换为定时器初值
% time_clk_fre_m 单位是Mhz
% sw_freq 单位是khz
% 这个函数输出的不再是Qnm格式的定点数，而是普通整型
function [tout,arra,arrb,arrc] = action_duration(n,a,b,c,udc, sw_freq, time_clk_fre_m)
    global QN_M;
    sqrt3_Q28 = 464943848;
    sqrt3 = sqrt3_Q28 / 2^(28 - QN_M);

    % ts的单位为us
    ts = (1000 * 2^QN_M) * 2^QN_M / sw_freq;
    udc_buf = sqrt3 * ts / udc;
    switch n
        case 3
            x_buf = udc_buf * b / 2^QN_M;
            y_buf = udc_buf * a / 2^QN_M;
        case 1
            x_buf = udc_buf * (-b) / 2^QN_M;
            y_buf = udc_buf * (-c) / 2^QN_M;
        case 5
            x_buf = udc_buf * a / 2^QN_M;
            y_buf = udc_buf * c / 2^QN_M;
        case 4
            x_buf = udc_buf * (-a) / 2^QN_M;
            y_buf = udc_buf * (-b) / 2^QN_M;
        case 6
            x_buf = udc_buf * -c / 2^QN_M;
            y_buf = udc_buf * b / 2^QN_M;
        case 2
            x_buf = udc_buf * -c / 2^QN_M;
            y_buf = udc_buf * -a / 2^QN_M;
    end
    ta = ( ts - x_buf - y_buf ) / 4;
    tb = ( ts + x_buf - y_buf ) / 4;
    tc = ( ts + x_buf + y_buf ) / 4;
    fprintf('ta * time_clk_fre_m = %.6f;tb * time_clk_fre_m = %.6f;tc * time_clk_fre_m = %.6f\n', ta * time_clk_fre_m,tb * time_clk_fre_m,tc * time_clk_fre_m);

    switch n
        case 3
            arra = round( ta * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrb = round( tb * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrc = round( tc * time_clk_fre_m / 2^QN_M / 2^QN_M );
        case 1
            arra = round( tb * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrb = round( ta * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrc = round( tc * time_clk_fre_m / 2^QN_M / 2^QN_M );
        case 5
            arra = round( tc * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrb = round( ta * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrc = round( tb * time_clk_fre_m / 2^QN_M / 2^QN_M );
        case 4
            arra = round( tc * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrb = round( tb * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrc = round( ta * time_clk_fre_m / 2^QN_M / 2^QN_M );
        case 6
            arra = round( tb * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrb = round( tc * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrc = round( ta * time_clk_fre_m / 2^QN_M / 2^QN_M );
        case 2
            arra = round( ta * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrb = round( tc * time_clk_fre_m / 2^QN_M / 2^QN_M );
            arrc = round( tb * time_clk_fre_m / 2^QN_M / 2^QN_M );
    end
    fprintf('arra = %.6f;arrb = %.6f;arrc = %.6f\n', arra,arrb,arrc);
    if arra < 20
        arra = 20;
    end
    if arrb < 20
        arrb = 20;
     end
    if arrc < 20
        arrc = 20;
    end

    tout = (time_clk_fre_m * ts) / 2^QN_M / 2^QN_M / 2;
    if arra > tout
        arra = tout - 20;
    end
    if arrb > tout
        arrb = tout - 20;
    end
    if arrc > tout
        arrc = tout - 20;
    end

    
end

% 这里与action_duration约定定时器的时钟是200M
% 不画图的版本
% function [uup,vup,wup,udown,vdown,wdown] = up_down_count(tout, arra, arrb, arrc)
%     uup = 0;
%     vup = 0;
%     wup = 0;
%     udown = 0;
%     vdown = 0;
%     wdown = 0;
% 
%     for count = 0:1:tout
%         if( count > arra - 20 )
%            udown = 0;
%         end
%         if( count > arra )
%            uup = 1;
%         end
% 
%         if( count > arrb - 20 )
%            vdown = 0;
%         end
%         if( count > arrb )
%            vup = 1;
%         end
% 
%         if( count > arrc - 20 )
%            wdown = 0;
%         end
%         if( count > arrc )
%            wup = 1;
%         end
%     end 
% 
%     for count = tout:-1:0
%         if( count < arra - 20 )
%            udown = 1;
%         end
%         if( count < arra )
%            uup = 0;
%         end
% 
%         if( count < arrb - 20 )
%            vdown = 1;
%         end
%         if( count < arrb )
%            vup = 0;
%         end
% 
%         if( count < arrc - 20 )
%            wdown = 1;
%         end
%         if( count < arrc )
%            wup = 0;
%         end
%     end 
% end
function up_down_count(tout, arra, arrb, arrc)
    % 扩展时间轴
    time = 0:(2 * tout);  % 从0到2*tout

    % 初始化信号存储
    uup_vals = zeros(1, 2 * tout + 1);
    vup_vals = zeros(1, 2 * tout + 1);
    wup_vals = zeros(1, 2 * tout + 1);
    udown_vals = zeros(1, 2 * tout + 1);
    vdown_vals = zeros(1, 2 * tout + 1);
    wdown_vals = zeros(1, 2 * tout + 1);

    % 初始化信号
    uup = 0;
    vup = 0;
    wup = 0;
    udown = 1;
    vdown = 1;
    wdown = 1;

    % 向上计数阶段
    for count = 0:1:tout
        if count > arra - 20
           udown = 0;
        end
        if count > arra
           uup = 1;
        end

        if count > arrb - 20
           vdown = 0;
        end
        if count > arrb
           vup = 1;
        end

        if count > arrc - 20
           wdown = 0;
        end
        if count > arrc
           wup = 1;
        end

        % 存储信号值
        uup_vals(count + 1) = uup;
        vup_vals(count + 1) = vup;
        wup_vals(count + 1) = wup;
        udown_vals(count + 1) = udown;
        vdown_vals(count + 1) = vdown;
        wdown_vals(count + 1) = wdown;
    end

    % 向下计数阶段
    for count = tout:-1:0
        if count < arra - 20
           udown = 1;
        end
        if count < arra
           uup = 0;
        end

        if count < arrb - 20
           vdown = 1;
        end
        if count < arrb
           vup = 0;
        end

        if count < arrc - 20
           wdown = 1;
        end
        if count < arrc
           wup = 0;
        end

        % 存储信号值到向下计数阶段
        uup_vals(2 * tout - count + 1) = uup;
        vup_vals(2 * tout - count + 1) = vup;
        wup_vals(2 * tout - count + 1) = wup;
        udown_vals(2 * tout - count + 1) = udown;
        vdown_vals(2 * tout - count + 1) = vdown;
        wdown_vals(2 * tout - count + 1) = wdown;
    end

    % 添加偏移量，使同相的信号更接近显示
    uup_vals = uup_vals + 6;
    udown_vals = udown_vals + 5;
    vup_vals = vup_vals + 3;
    vdown_vals = vdown_vals + 2;
    wup_vals = wup_vals + 0;
    wdown_vals = wdown_vals - 1;

    % 绘制图像
    figure;
    hold on;
    plot(time, uup_vals, 'DisplayName', 'uup');
    plot(time, udown_vals, 'DisplayName', 'udown');
    plot(time, vup_vals, 'DisplayName', 'vup');
    plot(time, vdown_vals, 'DisplayName', 'vdown');
    plot(time, wup_vals, 'DisplayName', 'wup');
    plot(time, wdown_vals, 'DisplayName', 'wdown');
    hold off;

    % 设置图表属性
    title('SVPWM Up/Down Signals with Full Up and Down Counting Periods');
    xlabel('Time');
    ylabel('Signal Value');
    legend;
    grid on;
end

% 没有微分
function u = pid(Kp, Ki, target, ref)
    persistent ek ek_1; % 使用 persistent 定义持久变量
    if isempty(ek)  % 检查是否第一次调用
        ek = 0;
        ek_1 = 0;
    end
    ek = target - ref;
    P = Kp * (ek - ek_1);
    I = Ki * ek;
    u = P + I;
    ek_1 = ek;
end

function test_pid()
    Kp = 1.14;      % 比例增益
    Ti = 1.35;
    Ki = Kp / Ti;      % 积分增益
    target = 10;   % 目标值
    ref = 0;       % 初始值
    load_factor = 0.5; % 负载系数，模拟响应延迟
    num_steps = 100; % 控制步数

    % 用于存储每一步的系统响应值和控制输出
    response = zeros(1, num_steps);
    control_output = zeros(1, num_steps);

    % 测试过程
    for i = 1:num_steps
        % 调用 PID 控制器
        u = pid(Kp, Ki, target, ref);
        
        % 负载模型：假设系统存在负载，使 ref 的变化受限
        ref = ref + load_factor * u;  % 加入负载因子对响应的影响
        
        % 记录系统响应和控制输出
        response(i) = ref;
        control_output(i) = u;
    end

    % 绘制系统响应曲线
    figure;
    subplot(2,1,1);
    plot(1:num_steps, response, '-');
    title('System Response');
    xlabel('Step');
    ylabel('Response (ref)');
    grid on;

    % 绘制控制输出曲线
    subplot(2,1,2);
    plot(1:num_steps, control_output, '-');
    title('Control Output (u)');
    xlabel('Step');
    ylabel('Control Output (u)');
    grid on;
end
